/*LICENSE_BEGIN
 * ******************************************************************
 * Copyright 2006 by the Dainamite Robocup-Team at the
 * DAI-Labor, TU-Berlin (www.dai-labor.de)
 * 
 * Members of Dainamite are:
 * Holger Endert
 * Robert Wetzker
 * Thomas Karbe
 * Axel Heßler
 * Philippe Bütter
 * Felix Brossmann
 * 
 * Licensed under the GNU GPL (General Public License) Free Software 
 * License, Version 2. You may use this file only in compliance with 
 * the License. You may obtain a copy of the License at:
 * 
 * http://www.fsf.org/licensing/licenses/gpl.txt
 * ******************************************************************
LICENSE_END*/
package robocup.component.geometry;

import java.awt.geom.Point2D;

/**
 * Simple Vector. Points at x,y. 
 * 
 * The angle to the y-axis is the vector's angle due to the dainamite
 * coordinate system (see Manual).
 */
public class Vektor {

	// statics
	/**
	 * creates an Vektor object. (same as "new Vektor()").
	 * 
	 * @return
	 */
	public static Vektor getDummyInstance() {
		return new Vektor();
	}

	/**
	 * creates an Array of Vektors
	 * 
	 * @param number
	 *            of Vektors
	 * @return an Vektor[] of lenght number
	 */
	public static Vektor[] getDummyInstances(int number) {
		Vektor[] vektors = new Vektor[number];
		for (int i = 0; i < vektors.length; i++) {
			vektors[i] = new Vektor();
		}
		return vektors;
	}

	// Constants for the constructors
	/**
	 * <code>POLAR</code> polar coordinates flag
	 */
	public final static int POLAR = 1;

	/**
	 * <code>XY</code> cartesian coordinates flag
	 */
	public final static int XY = 2;

	public double x;

	public double y;

	private double length;

	private double angle;

	// the coordinate center
	public static final Vektor CENTER = new Vektor();

	/**
	 * Zero-vector
	 */
	public Vektor() {
		this.x = 0;
		this.y = 0;
	}

	/**
	 * Vektor from polar coordinates. <code>iAngle</code> is not the usual
	 * vector angle, but (90� - vector angle) - the angle (in degrees) to the
	 * Y-axis.
	 * 
	 * @param iLength -
	 *            length of the vector
	 * @param iAngle -
	 *            angle (in degrees) to Y-axis
	 */
	public Vektor(double iLength, double iAngle) {
		this.y = iLength * Math.cos(iAngle * (Math.PI / 180));
		this.x = iLength * Math.sin(iAngle * (Math.PI / 180));
	}

	/**
	 * Vektor created from polar or cartesian coordinates
	 * 
	 * @param type -
	 *            polar/cartesian
	 * @param XorLength -
	 *            X-coordinate or length of the vector
	 * @param YorAngle -
	 *            Y-coordinate or angle (in degrees) of the vector
	 */
	public Vektor(int type, double XorLength, double YorAngle) {
		if (type == XY) { // XY-koordinaten
			this.x = XorLength;
			this.y = YorAngle;
		} else {// Polarkoordinaten
			this.y = XorLength * Math.cos(YorAngle * (Math.PI / 180));
			this.x = XorLength * Math.sin(YorAngle * (Math.PI / 180));
		}
	}

  /**
   * sets the vektor's data avoiding the creation of a new instance.
   * 
   * @param dist
   * @param angle
   */
	public void setVektor(double dist, double angle) {
		this.y = dist * Math.cos(angle * (Math.PI / 180));
		this.x = dist * Math.sin(angle * (Math.PI / 180));
	}

	/**
	 * Clones a Vektor without creating a new one.
	 * 
	 * @param v
	 */
	public void copy(Vektor v) {
		this.x = v.x;
		this.y = v.y;
	}

	/**
	 * sets the Vektor to (0,0)
	 * 
	 */
	public void reset() {
		this.x = 0;
		this.y = 0;
	}

	/**
	 * <code>Vektor</code> constructor creates a Vektor from a 2D point.
	 * 
	 * @param point
	 *            Point2D representation of the vector
	 */
	public Vektor(Point2D point) {
		this(XY, point.getX(), point.getY());
	}

	/**
	 * This method normalizes an angle.
	 * 
	 * @param angle
	 *            The angle to normalize.
	 * @return An angle with -180 <= angle <= 180.
	 */
	public static double normalize(double angle) {
		while (angle < -180)
			angle += 360;
		while (angle > 180)
			angle -= 360;
		return angle;
	}

	/**
	 * pointAt is a setter for the coordinates <code>x</code> & <code>y</code>
	 * 
	 * @param x_value
	 * @param y_value
	 */
	public final void pointAt(double x_value, double y_value) {
		this.x = x_value;
		this.y = y_value;
	}

	/**
	 * set the Vektor by polar data.
	 * 
	 * @param length
	 * @param angle
	 */
	public final void pointAtPolar(double length, double angle) {
		this.y = length * Math.cos(angle * (Math.PI / 180));
		this.x = length * Math.sin(angle * (Math.PI / 180));
	}

	/**
	 * mult multiplies the coordinates of the Vektor with factor <code>a</code>
	 * 
	 * @param a - factor
	 */
	public Vektor mult(double a) {
		this.x *= a;
		this.y *= a;
		return this;
	}
  

  /**
   * as mult but returns the result without changing the 
   * instance itself.
   * 
   * @param a -
   *            factor
   * @return new Vektor with same dir but length = old_length * a;
   */
  public Vektor mult2(double a) {
    return new Vektor(Vektor.XY, this.x * a, this.y * a);
  }

	/**
	 * <code>div</code> divides the coordinates of this Vektor
	 * 
	 * @param divider
	 *            divide factor
	 */
	public void div(double divider) {
		this.x /= divider;
		this.y /= divider;
	}

	/**
	 * <code>div2</code>
	 * 
	 * @param divider
	 * @return Vektor
	 */
	public Vektor div2(double divider) {
		return new Vektor(Vektor.XY, this.x / divider, this.y / divider);
	}

	/**
	 * sub returns a new Vektor equal to <code>this</code> - <code>v</code>
	 * 
	 * @param v -
	 *            Vektor to substract
	 * @return Vektor - <code>this</code> - <code>v</code>
	 */
	public final Vektor sub(Vektor v) {
		return new Vektor(XY, this.x - v.x, this.y - v.y);
	}

	/**
	 * sub returns <code>this</code> after <code>v</code> was sustracted
	 * from <code>this</code>. No new vektor instance is created.
	 * 
	 * @param v -
	 *            Vektor to substract
	 * @return Vektor - <code>this</code> - <code>v</code>
	 */
	public final Vektor subFromThis(Vektor v) {
		this.x -= v.x;
		this.y -= v.y;
		return this;
	}

	/**
	 * add returns a new Vektor equal to <code>this</code> + <code>v</code>
	 * 
	 * @param v -
	 *            Vektor to add
	 * @return Vektor - <code>this</code> + <code>v</code>
	 */
	public final Vektor add(Vektor v) {
		return new Vektor(XY, this.x + v.x, this.y + v.y);
	}

	/**
	 * addToThis returns <code>this</code> Vektor after <code>v</code> was
	 * added to this. No new vektor instance is created.
	 * 
	 * @param v -
	 *            Vektor to add
	 * @return Vektor - <code>this</code> + <code>v</code>
	 */
	public final Vektor addToThis(Vektor v) {
		this.x += v.x;
		this.y += v.y;
		return this;
	}

	/**
	 * returns the distance to a given vektor
	 * 
	 * @param v
	 * @return distance to v
	 */
	public final double getDistance(Vektor v) {
		return Math.sqrt((this.x - v.x) * (this.x - v.x) + (this.y - v.y)
				* (this.y - v.y));
	}

	/**
	 * cross returns the cross product with an vektor
	 * 
	 * @param 2.
	 *            Vektor
	 * @return cross product
	 */
	public double cross(Vektor v) {
		return (this.x * v.y - this.y * v.x);
	}

	/**
	 * scalar of 2 Vektors
	 * 
	 * @param v
	 * @return
	 */
	public double scalar(Vektor v) {
		return this.x * v.x + this.y * v.y;
	}

	/**
	 * getLength returns the length of the vector
	 * 
	 * @return double - length
	 */
	public final double getLength() {
		return (Math.sqrt(this.x * this.x + this.y * this.y));
	}

	/**
	 * getAngle returns the angle (in degrees) of the vector to the Y-Axis
	 * Absolute: Angle between the vector and the goal-goal-line 
	 * 
	 * @return double - angle to Y-axis in degree
	 */
	public double getAngle() {
		if (this.x == 0.0 && this.y == 0) {
			return 0.0;
		}
		return Math.atan2(this.x, this.y) * (180.0 / Math.PI);
	}

	/**
	 * Returns a Vektor from this Vektor to the global Vektor <code>v</code>.
	 * Note that this method is equal to <code>v</code>.sub(<code>this</code>).
	 * 
	 * @param v -
	 *            global Vektor
	 * @return Vektor - from this Vektor to the global Vektor <code>v</code>
	 */
	public final Vektor getVektorTo(Vektor v) {
		return v.sub(this);
	}

	/**
	 * Returns the global angle to the specified Vektor <code>v</code>
	 * relative to the position denoted by this Vektor.
	 * 
	 * @param v -
	 *            global Vektor
	 * @return double - global angle from this Vektor to the global position
	 *         specified by the Vektor <code>v</code>.
	 */
	public final double getAngleTo(Vektor v) {
		if (this.x == v.x && this.y == v.y) {
			return 0.0;
		}
		return Math.atan2(v.x - this.x, v.y - this.y) * (180.0 / Math.PI);
	}

	/**
	 * Returns the global angle between the angle of this Vektor and the angle
	 * of the specified Vektor <code>v</code>.
	 * 
	 * @param v -
	 *            Vektor
	 * @return double - global angle between both vektors
	 */
	public final double getAngleBetween(Vektor v) {
		return Math.abs(Vektor.normalize(this.getAngle() - v.getAngle()));
	}

	/**
	 * returns the angle between to positions seen from the Vektor.
	 * 
	 * @param v1 -
	 *            left position
	 * @param v2 -
	 *            right position
	 * @return angle between v1 and v2 seen from this vektor
	 */
	public final double getAngleBetween(Vektor v1, Vektor v2) {
		return Vektor.normalize(this.getAngleTo(v2) - this.getAngleTo(v1));
	}

	/**
	 * This method returns a Vektor that is interjacent to the Vektor and the
	 * given Vektor <code>v</code>.
	 * 
	 * @param v
	 * @return
	 */
	public Vektor getVektorBetween(Vektor v) {
		return new Vektor(Vektor.XY, (this.x + v.x) / 2, (this.y + v.y) / 2);
	}

	/**
	 * setLength sets the length of the vector to <code>length</code>
	 * 
	 * @param length -
	 *            new length
	 */
	public Vektor setLength(double length) {
		double thisLength = this.getLength();
		if (thisLength == 0) {
			if (length == 0) {
				return this;
			} else {
				this.x = 0;
				this.y = length;
				return this;
			}
		}

		double rel = length / thisLength;
		this.x *= rel;
		this.y *= rel;
		return this;
	}

	/**
	 * Rotates the vector at the angle of alpha
	 * 
	 * @param alpha -
	 *            angle of the rotation
	 */
	public void rotate(double alpha) {
		this.length = this.getLength();
		this.angle = Math.toRadians(this.getAngle() + alpha);
		this.y = this.length * Math.cos(this.angle);
		this.x = this.length * Math.sin(this.angle);

		// Vektor temp = new Vektor(this.getLength(), this.getAngle() + alpha);
		// this.x = temp.x;
		// this.y = temp.y;
	} // end of method rotate(int alpha)

	/**
	 * Clones the Vektor.
	 * 
	 * @return Clone
	 */
	public Vektor cloned() {
		return new Vektor(Vektor.XY, this.x, this.y);
	}

	/**
	 * sets the Vektor to "empty"
	 * 
	 */
	public void setEmpty() {
		this.x = java.lang.Double.MAX_VALUE;
	}

	/**
	 * tests if Vektor is "empty"
	 * 
	 * @return
	 */
	public boolean isEmpty() {
		return this.x == java.lang.Double.MAX_VALUE;
	}

	/*
	 * (non-Javadoc)
	 * 
	 * @see robocup.component.infotypes.Infos#toString()
	 */
	@Override
	public String toString() {
		return ("(" + Math.rint(this.x * 100) / 100 + ", "
				+ Math.rint(this.y * 100) / 100 + ")");
	}

	/**
	 * toSting() with only n digits for doubles
	 * 
	 * @param n -
	 *            Number of digits
	 * @return
	 */
	public String toString(int n) {
		double a = Math.pow(10.0, n);
		return ("(" + Math.rint(this.x * a) / a + ", " + Math.rint(this.y * a)
				/ a + ")");
	}

	/**
	 * <code>getPoint</code> returns a Point2D representation of the Vektor
	 * 
	 * @return Point2D
	 */
	public final Point2D getPoint() {
		return new Point2D.Double(this.x, this.y);
	}

	/**
	 * checks if the given vektor <code>v</code> lies in a environment with
	 * size of <code>variation</code>
	 * 
	 * @param v
	 * @param variation
	 */
	public boolean compareWithVariation(Vektor v, double variation) {
		return (getDistance(v) <= variation);
	}

	/**
	 * @return Returns the x.
	 */
	public double getX() {
		return this.x;
	}

	/**
	 * @param x
	 *            The x to set.
	 */
	public void setX(double x) {
		this.x = x;
	}

	/**
	 * @return Returns the y.
	 */
	public double getY() {
		return this.y;
	}

	/**
	 * @param y
	 *            The y to set.
	 */
	public void setY(double y) {
		this.y = y;
	}
} // end of class Vektor
